
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       Imu.h
  * @author     baiyang
  * @date       2021-7-14
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <common/gp_math/gp_mathlib.h>
#include <common/filter/lpfilter.h>
#include <stdlib.h>

/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
//输入dt 电机转速rad/s 
typedef struct
{
    //bool vibration_enable;    //是否使能
    //float integrator;         //积分器
    //参数
    float Weight;
    float RPM_Max;
    float Gain;
}Vibration;

typedef struct
{
    Vibration tVib;            //震动参数
    float* Integtrator;
    uint8_t num;
    LowPassFilt_vec3f imu_filt;

    Vector3f_t scale_error;    //偏差量
    Vector3f_t base_error;
    Vector3f_t variance;    //高斯白噪声方差

    bool enable_vib;        //是否使能震动干扰
    bool enable_err;        //是否使能偏差
}ImuMode;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void ImuMode_Update(ImuMode* pImu, Vector3f_t* pState_out, const Vector3f_t* pState_in, float* prop_vel, float dt);
void ImuMode_Init(ImuMode* pImu);

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif

